#include <planner/jps_planner/jps.h>
#include "map/grid_map.h"

JPS::JPS(bool verbose) : planner_verbose_(verbose) {
  planner_verbose_ = verbose;
  if (planner_verbose_)
    printf(ANSI_COLOR_CYAN "sz PLANNER VERBOSE ON\n" ANSI_COLOR_RESET);
}

void JPS::setMapUtil(const std::shared_ptr<sz::NavigationMap> &map_util) {
  map_util_ = map_util;
}

int JPS::status() const { return status_; }

__attribute__((unused)) VectorPoint2f JPS::GetRawPath() { return raw_path_; }
VectorPoint2i JPS::GetMapRawPath() { return map_raw_path_; }

__attribute__((unused)) VectorPoint2f JPS::GetOpenSet() const {
  VectorPoint2f ps;
  const auto ss = graph_search_->getOpenSet();
  for (const auto &it: ss) {
    ps.push_back(map_util_->MapToWorld(Point2i(it->x, it->y)));
  }
  return ps;
}

__attribute__((unused)) VectorPoint2f JPS::GetCloseSet() const {
  VectorPoint2f ps;
  const auto ss = graph_search_->getCloseSet();
  for (const auto &it: ss) {
    ps.emplace_back(map_util_->MapToWorld(Point2i(it->x, it->y)));
  }
  return ps;
}

__attribute__((unused)) VectorPoint2f JPS::GetAllSet() const {
  VectorPoint2f ps;
  const auto ss = graph_search_->getAllSet();
  for (const auto &it: ss) {

    ps.push_back(map_util_->MapToWorld(Point2i(it->x, it->y)));
  }
  return ps;
}

bool JPS::Plan(const Pointf<2> &start, const Pointf<2> &goal, float_decimal_t eps) {
  if (planner_verbose_) {
    std::cout << "Start: " << start.transpose() << std::endl;
    std::cout << "Goal:  " << goal.transpose() << std::endl;
    std::cout << "Epsilon:  " << eps << std::endl;
  }

//  path_.clear();
  raw_path_.clear();
  status_ = 0;

  const Point2i start_int = map_util_->WorldToMap(start);
  if (!map_util_->IsFree(start_int)) {
    if (planner_verbose_) {
      if (map_util_->IsBumperOccupied(start_int))
        printf(ANSI_COLOR_RED "start is bumper occupied!\n" ANSI_COLOR_RESET);
      else if (map_util_->IsSLAMOccupied(start_int))
        printf(ANSI_COLOR_RED "start is SLAMSystem occupied!\n" ANSI_COLOR_RESET);
      else {
        printf(ANSI_COLOR_RED "start is outside!\n" ANSI_COLOR_RESET);
        std::cout << "startI: " << start_int.transpose() << std::endl;
        std::cout << "Map origin: " << map_util_->GetMax().transpose() << std::endl;
        std::cout << "Map dim: " << map_util_->GetDim().transpose() << std::endl;
      }
    }
    status_ = 1;
    return false;
  }

  const Point2i goal_int = map_util_->WorldToMap(goal);
  if (!map_util_->IsFree(goal_int)) {
    if (planner_verbose_)
      printf(ANSI_COLOR_RED "goal is not free!\n" ANSI_COLOR_RESET);
    status_ = 2;
    return false;
  }

  const Point2i &dim = map_util_->GetDim();

  graph_search_ =
      std::make_shared<sz::JPSGraphSearch>(map_util_->GetMap().data(), dim(0), dim(1), eps, planner_verbose_);
  graph_search_->plan(start_int(0), start_int(1), goal_int(0), goal_int(1), 2000);

  map_raw_path_ = graph_search_->getPath();
  if (map_raw_path_.empty()) {
    if (planner_verbose_)
      std::cout << ANSI_COLOR_RED "Cannot find a path from " << start.transpose() << " to " << goal.transpose()
                << " Abort!" ANSI_COLOR_RESET << std::endl;
    status_ = -1;
    return false;
  }


  //**** raw path, s --> g
  VectorPoint2f ps;
  for (const auto &point : map_raw_path_)
    ps.emplace_back(map_util_->MapToWorld(point));
  raw_path_.swap(ps);
  return true;
}




